//
// Created by 张祎鹤严 on 2023/9/24.
//

#include "driver_vision.h"

extern GyroDataStruct GyroDataSave;
extern StampDataStruct TimeStampedData;
AutoAimingParaStruct AutoAimingPara={0, 28, 30}; //颜色，弹速，弹速上限  //red:0 blue:1  自己的颜色
extern rc_device_t g_rc;
extern uint8_t is_vision_shoot_permit;
extern judge_info_0 judgeInfo0;
extern judge_info_1 judgeInfo1;
extern judge_info_2 judgeInfo2;
extern enemy_coo enemyCoo;
extern sentry_state normal_state;
extern uint8_t outpost_not_spin;
extern uint32_t fire_interval_time;


float Gyro_Yaw_Align;
float Gyro_Pitch_Align;
VisionReceiveDataStruct VisionReceiveData={0};
kalman_filter_t vision_yaw_kalman_filter={0};
kalman_filter_t vision_pitch_kalman_filter={0};
kalman_filter_t vision_z_kalman_filter={0};

kalman_filter_init_t vision_yaw_change_kalman_filter_para = {
        .xhat_data = {0, 0},
        .P_data = {2, 0, 0, 2},
        .A_data = {1, Dt, 0, 1},
        .H_data = {1, 0, 0, 1},
        .Q_data = {1, 0, 0, 1},
        .R_data = {200.0f, 0, 0, 0.1f}
};
kalman_filter_init_t vision_pitch_change_kalman_filter_para ={
        .xhat_data = {0, 0},
        .P_data = {2, 0, 0, 2},
        .A_data = {1, Dt, 0, 1},
        .H_data = {1, 0, 0, 1},
        .Q_data = {1, 0, 0, 1},
        .R_data = {80, 0, 0, 0.1f}// 5000越小越贴近测量值
};
kalman_filter_init_t vision_z_change_kalman_filter_para ={
        .xhat_data = {0, 0},
        .P_data = {2, 0, 0, 2},
        .A_data = {1, Dt, 0, 1},
        .H_data = {1, 0, 0, 1},
        .Q_data = {1, 0, 0, 1},
        .R_data = {150, 0, 0, 5}// 5000越小越贴近测量值
};


GimbalCtrl VisionGimbalCtrl;
AimAutoStruct AimAutoData;
/**
 * @brief Init the Transformation matrix from camera to ballistic //TODO: write in ros tf
 * @param x Translate x, 单位mm
 * @param y Translate y, 单位mm
 * @param z Translate z, 单位mm
 * @param pitch 摄像头与枪管的pitch角度差, 单位角度（deg）
 * @param yaw 摄像头与枪管的yaw角度差, 单位角度（deg）
 * @param init_v 初速度，单位m/s
 * @param init_k 空气摩擦因数，默认为0.1
 */

uint32_t GimbalReactTime;
void VisionInit() {
    VisionGimbalCtrl.offset_x = OFFSET_X;
    VisionGimbalCtrl.offset_y = OFFSET_Y;
    VisionGimbalCtrl.offset_z = OFFSET_Z;
    VisionGimbalCtrl.init_k = VISION_K;
    VisionGimbalCtrl.init_v = VISION_V;
    VisionGimbalCtrl.offset_yaw = VISION_YAW;
    VisionGimbalCtrl.offset_pitch = VISION_PITCH;

//    kalman_filter_init(&vision_yaw_kalman_filter, &vision_yaw_change_kalman_filter_para);
//    kalman_filter_init(&vision_pitch_kalman_filter, &vision_pitch_change_kalman_filter_para);
//    kalman_filter_init(&vision_z_kalman_filter, &vision_z_change_kalman_filter_para);

    VisionGimbalCtrl.shoot_delay = SHOOT_DELAY_TIME;
    GimbalReactTime = GimbalReacTime;
    AimAutoData.AutoAimStatus = 0;//AutoAim
    AimAutoData.AutoAim = 1;

}

void crc16_vision_update(uint16_t *currectCrc, const uint8_t *src, uint32_t lengthInBytes) {
    uint32_t crc = *currectCrc;
    uint32_t j;
    for (j=0; j < lengthInBytes; ++j)
    {
        uint32_t i;
        uint32_t byte = src[j];
        crc ^= byte << 8;
        for (i = 0; i < 8; ++i)
        {
            uint32_t temp = crc << 1;
            if (crc & 0x8000)
            {
                temp ^= 0x1021;
            }
            crc = temp;
        }
    }
    *currectCrc = crc;
}

VisionDataStruct VisionReceive={0},VisionTransmit={0},VisionReceiveLast={0};
uint16_t CrcVisionSendTest;
uint16_t debugshootspeed = 280;

void TransmitData(VisionDataStruct *VisionTransmitDataTemp, uint8_t *UsbTxBufTemp){
    AutoAimingPara.color_flag = judgeInfo1.is_blue;
    CrcVisionSendTest = 0;
    VisionTransmitDataTemp->vision_data.head = 0X72;
    //PITCH上+下-，YAW左+右-
    if(AimAutoData.AutoAimStatus ==1||AimAutoData.AutoAimStatus == 3){
        /****Gyro AutoAim & Gyro AutoAimEnergy****/
        VisionTransmitDataTemp->vision_data.horizon_x = TimeStampedData.GyroData.Yaw;

    }else {
        VisionTransmitDataTemp->vision_data.horizon_x =
                (TimeStampedData.GyroData.Yaw) * 2 * PI;
    }

    VisionTransmitDataTemp->vision_data.vertical_y = (TimeStampedData.GyroData.Pitch - PITCH_GYRO_INIT_VALUE_SET) * PI;

    if(AutoAimingPara.bullet_actual_speed!=0)
        VisionTransmitDataTemp->vision_data.ser_z = (uint16_t)(AutoAimingPara.bullet_actual_speed * 10.0f);
    else
        VisionTransmitDataTemp->vision_data.ser_z = debugshootspeed;
    VisionTransmitDataTemp->vision_data.pre_time_stamp = TimeStampedData.time_stamp;

    if(AutoAimingPara.color_flag == 1){//blue 5
        //buff red aim
#if GO_ATTACK_OUTPOST_DEBUG
        VisionTransmitDataTemp->vision_data.armor_number = 5 + GO_ATTACK_OUTPOST_DEBUG;
#else
        VisionTransmitDataTemp->vision_data.armor_number = 5 + (normal_state == go_attack_outpost ? 1 : 0) + (normal_state == go_attack_outpost ? outpost_not_spin : 0);
//        VisionTransmitDataTemp->vision_data.armor_number = 5;
#endif
    }
    else{//red 0
#if GO_ATTACK_OUTPOST_DEBUG
        VisionTransmitDataTemp->vision_data.armor_number = GO_ATTACK_OUTPOST_DEBUG;
#else
        VisionTransmitDataTemp->vision_data.armor_number = 0 + (normal_state == go_attack_outpost ? 1 : 0) + (normal_state == go_attack_outpost ? outpost_not_spin : 0);
//        VisionTransmitDataTemp->vision_data.armor_number = 0;
#endif
    }
//    VisionTransmitDataTemp->vision_data.armor_number = 5;
    VisionTransmitDataTemp->vision_data.firing_rate = 0;
    crc16_vision_update(&CrcVisionSendTest, VisionTransmitDataTemp->data, 61);
    VisionTransmitDataTemp->vision_data.crc = CrcVisionSendTest;
    VisionTransmitDataTemp->vision_data.tail = 0X4C;
    for(int i=0;i<64;i++){
        UsbTxBufTemp[i]=VisionTransmitDataTemp->data[i];
    }
}


uint16_t CrcVisionReceiveTest;

uint8_t VisionShootPermit;

uint32_t AimAutoReceiveCount = 0;
float AimAutoReceiveDelta;
void ReceiveData(const uint8_t *Buffer){

    for(int i =0;i<64;i++) {
        VisionReceive.data[i] = Buffer[i];
    }
    CrcVisionReceiveTest=0;
    crc16_vision_update(&CrcVisionReceiveTest, VisionReceive.data, 61);
    if(VisionReceive.vision_data.crc != (CrcVisionReceiveTest) || (VisionReceive.vision_data.armor_number == 100)){
        for(int j=0;j<64;j++){
            VisionReceive.data[j] = 0;
        }
        VisionReceive.vision_data.armor_number = 100;
        return ;
    }
//    if(VisionReceieveTest.VisionTest.status!=VisionReceieveLastTest.VisionTest.status){
//        VisionReceieveTest.VisionTest.horizon_x =0;
//    }

    if(VisionReceive.vision_data.armor_axis_x != VisionReceiveLast.vision_data.armor_axis_x)
        AimAutoReceiveDelta = (float)(1.0f / DWT_GetDeltaT(&AimAutoReceiveCount));

    for(int k=0;k<64;k++){
        VisionReceiveLast.data[k] = VisionReceive.data[k];
    }
    VisionShootPermit = 1;
/**
 * 弹频限制，正常情况记得打开
 * debug时注释掉
 **/
//    if(VisionReceieveTest.VisionTest.armor_flag == 10)
//        shootdelay = 3;
//    else if(VisionReceieveTest.VisionTest.armor_flag == 11 || VisionReceieveTest.VisionTest.armor_flag == 12)
//        shootdelay = 20;
/****************************************/

//    if(VisionReceieveTest.VisionTest.pre_time_stamp == VisionReceieveLastTest.VisionTest.pre_time_stamp){
//        for(int k = 0;k<64;k++){
//            VisionReceieveTest.data[k] = 0;
//        }
//    }

}

Coordinate coordinate_w2c;

/**
 * @fn BulletModel:定义了SIGNGLE_DIRECTION是，定义是单向空气阻力模型，否则为双向空气阻力模型
 * @brief 完全的弹道模型（考虑x与y方向上的空气阻力）
 * @param x x方向上的位移（敌方装甲板到枪口的距离，tVec的[0]位置）
 * @param v 子弹出射初速度
 * @param angle 迭代当前pitch角度
 * @return 本次迭代所计算的pitch实际值（位置）
 */

float BulletModel(float x, float v, float angle) {
    float a=1-VisionGimbalCtrl.init_k * x / (v * cosf(angle));
    if(a<=0)
        a=1e-6;
    return x * GRAVITY / (VisionGimbalCtrl.init_k * v * cosf(angle)) + tanf(angle) * x +
           1 / (VisionGimbalCtrl.init_k * VisionGimbalCtrl.init_k) * GRAVITY * logf(a);
}


/** @brief 以上弹道模型应该是有错误的 @ref GetPitch, 由于PnP求出的(x, y, z)定义引起
 * @param pit 云台当前pitch， 水平时为0, 向上为正
 * @param xyz 最佳装甲板PnP解算结果（按照相机坐标系的定义）
 * @param v 子弹速度（摩擦轮 + 底盘平移运动在平行于枪管上的分量）
 * @param t 输出 子弹击中目标延迟
 * @return pitch增量
 */
float ypos;
float dist1;
float pitch_temp;
float ang;
PNP_Get_Pitch_Struct PNP_Get_Pitch_Data;
float pitwatch;
uint8_t bugpit;
float pnpGetPit(float pit, float x, float y, float z, float v, float* t) {
    pitch_temp = pit / RAD2DEG;//pitch角转弧度
    PNP_Get_Pitch_Data.pitch_temp = pitch_temp;
    float d = sqrtf(powf(x, 2) + powf(z, 2)); // 过渡量
    dist1 = fabsf(y * sinf(pitch_temp) + d * cosf(pitch_temp));

    //状态解算--相机坐标系到水平坐标系--target水平距离
    PNP_Get_Pitch_Data.dist = sqrtf(powf((-y*sinf(pitch_temp)+z*cosf(pitch_temp)),2)+powf(x,2));

    float dy ;
    ang = pitch_temp;
    PNP_Get_Pitch_Data.y_temp = PNP_Get_Pitch_Data.dist * tanf(pitch_temp);
    //状态解算--相机坐标系到水平坐标系--target竖直距离
//    PNP_Get_Pitch_Data.y_pos = y*cosf(pitch_temp) - z*sinf(pitch_temp);
    PNP_Get_Pitch_Data.y_pos = -y*cosf(pitch_temp) - z*sinf(pitch_temp);
    //PNP_Get_Pitch_Data.y_pos = y*cos(pitch_temp);
    ypos = d * sinf(pitch_temp) - y * cosf(pitch_temp);

    for (int i = 0; i < 25; ++i)
    {
        ang = atan2f(PNP_Get_Pitch_Data.y_temp, PNP_Get_Pitch_Data.dist);
        PNP_Get_Pitch_Data.y_actual =  BulletModel(PNP_Get_Pitch_Data.dist, v, ang);
        dy = - PNP_Get_Pitch_Data.y_pos - PNP_Get_Pitch_Data.y_actual;
        PNP_Get_Pitch_Data.y_temp += dy;
        if (fabsf(dy) < 0.001)
        {
            break;
        }
    }
//    *t =0;//(float)(-1/(VisionGimbalCtrl.init_k) * logf(1-VisionGimbalCtrl.init_k * PNP_Get_Pitch_Data.dist/(v*cosf(ang))));
//    if((fabsf(*t)>0.6f)|| (isnanf(*t))){
//        *t = 0 ;
//    }
    PNP_Get_Pitch_Data.pitch_target = ang * RAD2DEG;
    PNP_Get_Pitch_Data.pitch_change = PNP_Get_Pitch_Data.pitch_target - pit;
    return PNP_Get_Pitch_Data.pitch_change;
}

/**
 * @brief Get the ballistic control info.
 * @param postion Enemy position(actually it should be the target armor).
 * @param pitch Input and output actual pitch angle
 * @param yaw Input and output actual yaw angle
 * @param delay shoot delay while the ball is in the air towards the target
 * 所有的输入参数中，position的单位是mm,  pitch，yaw的单位是角度（deg）
 */
float TransformDelayTime = 2.8;
float omega_delay_time = 95;
float robot_axis_z_speed_filtered = 0;
void Transform(Point3fStruct position, float inPitch, float inYaw, float* pitch, float* yaw,
               float* delay, float trans_vel, float forward_vel)//FOWARD是子弹速度+前进分量
{
    *delay = TransformDelayTime;
    //y取负数是因为，图像中tVec为正时，装甲板在摄像头正对处下方
    //如果车在z轴方向存在运动，则加上运动速度forward_vel
    *pitch = pnpGetPit(inPitch, position.x / 1000, (position.y - VisionGimbalCtrl.offset_y) / 1000, (position.z + VisionGimbalCtrl.offset_z) / 1000,
                       forward_vel+VisionGimbalCtrl.init_v, delay) + VisionGimbalCtrl.offset_pitch;
    //yaw轴是逆时针为正
    //使用横向速度trans_vel时注意，向右为正（与相机坐标系一致）,则在角度解算时，子弹飞行时间车本身会位移，x需要- 速度 × 时间
    low_pass(robot_axis_z_speed_filtered, VisionReceive.vision_data.robot_axis_z_speed, 0.1);
    *yaw = -atan2f(position.x + VisionGimbalCtrl.offset_x - trans_vel * (*delay) - VisionReceive.vision_data.robot_axis_z_speed * omega_delay_time, position.z + VisionGimbalCtrl.offset_z) * RAD2DEG
           + VisionGimbalCtrl.offset_yaw;
//	*delay =*delay / 0.375f;//同系统时钟保持一致
    if (fabsf(*yaw) > 50|| isnanf(*yaw))
    {
        *yaw = 0;
    }
}

Point3fStruct Point3f_Perdicted_WorldAxis;
BulletSpeed SmallBulletSpeed;


/**
 *
 * @return 返回当前实时弹速
 * 读裁判系统弹速
 */
float GetBulletSpeed(){
    SmallBulletSpeed.SmallBulletSpeedFiltered = AutoAimingPara.bullet_actual_speed;
    SmallBulletSpeed.SmallBulletSpeedLimit = AutoAimingPara.bullet_limit_speed;

    if(SmallBulletSpeed.SmallBulletSpeedFiltered<10||SmallBulletSpeed.SmallBulletSpeedFiltered>30){
        if(SmallBulletSpeed.SmallBulletSpeedLimit == 15.f){
            SmallBulletSpeed.SmallBulletSpeedFiltered = 13.f;
        } else if(SmallBulletSpeed.SmallBulletSpeedLimit == 18.f){
            SmallBulletSpeed.SmallBulletSpeedFiltered = 16.f;
        } else{
            SmallBulletSpeed.SmallBulletSpeedFiltered = 28.f;
        }
    }
    return SmallBulletSpeed.SmallBulletSpeedFiltered;
}


uint32_t shoot_permit_cnt = 0;
void vision_shoot_permit(){
#if FORCE_NOT_FIRE
    is_vision_shoot_permit = 0;
#else

    if((distance_to_sentry(enemyCoo.x, enemyCoo.y) < RANGE_SCOPE && judgeInfo0.game_progress != ready_stage && judgeInfo0.game_progress != countdown_stage) && fire_interval_time >= MIN_FIRE_INTERVAL){
        if(VisionReceive.vision_data.firing_rate == HIGH_SHOOT_FREQ || VisionReceive.vision_data.firing_rate == MID_SHOOT_FREQ || VisionReceive.vision_data.firing_rate == LOW_SHOOT_FREQ){
            shoot_permit_cnt++;
            if(shoot_permit_cnt >= 1){
                is_vision_shoot_permit = 1;
                shoot_permit_cnt = 0;
            }
        }else{
            is_vision_shoot_permit = 0;
        }
    }else{
        is_vision_shoot_permit = 0;
    }


#endif

}


uint32_t AimAutoCount = 0;
float AimAutoDelta = 0;

float VisionChangeYaw;
float VisionChangePitch;
float VisionYSpeed;

extern float pitch_set_gyro;
extern float yaw_set_gyro;
float yaw_set_last = 0;
float pitch_set_last = 0;

float GimbalYawSetTemp;
float GimbalPitchSetTemp;

extern uint8_t VisionControlFlag;
float test_yaw;
float test_pitch;
void AimAuto() {
    //filter
//    VisionDataProcess();
    AimAutoDelta = (float)(1.0f / DWT_GetDeltaT(&AimAutoCount));

    Gyro_Yaw_Align = ((VisionReceive.vision_data.vertical_y) / 2 / PI - (int)((VisionReceive.vision_data.vertical_y) / 2 / PI)) * 360.0f;

    Gyro_Pitch_Align = VisionReceive.vision_data.vertical_y / PI + PITCH_GYRO_INIT_VALUE_SET;

    ///坐标变换及卡尔曼滤波
    coordinate_w2c.Axis_Camera.x = -VisionReceive.vision_data.armor_axis_x;
    coordinate_w2c.Axis_Camera.y = -VisionReceive.vision_data.armor_axis_y;
    coordinate_w2c.Axis_Camera.z = VisionReceive.vision_data.armor_axis_z;

    VisionYSpeed = -VisionReceive.vision_data.robot_axis_y_speed;
//    coordinate_w2c.Axis_Camera.x = -VisionPreTime[front].VisionTest.armor_axis_x;
//    coordinate_w2c.Axis_Camera.y = -VisionPreTime[front].VisionTest.armor_axis_y;
//    coordinate_w2c.Axis_Camera.z = VisionPreTime[front].VisionTest.armor_axis_z;
#if 0
    float *vision_yaw_kf_result = kalman_filter_calc(&vision_yaw_kalman_filter,  coordinate_w2c.Axis_Camera.x , 0);
    float *vision_z_kf_result = kalman_filter_calc(&vision_z_kalman_filter,coordinate_w2c.Axis_Camera.z,0);
    float *vision_pitch_kf_result = kalman_filter_calc(&vision_pitch_kalman_filter,coordinate_w2c.Axis_Camera.y,0);
    float x_predict = vision_yaw_kf_result[0];
    float y_predict = vision_pitch_kf_result[0];
    float z_predict = vision_z_kf_result[0];
#endif

#if 1
    float x_predict = coordinate_w2c.Axis_Camera.x;
    float y_predict = coordinate_w2c.Axis_Camera.y;
    float z_predict = coordinate_w2c.Axis_Camera.z;
#endif
    Point3f_Perdicted_WorldAxis.x = x_predict;//coordinate_c2w.Axis_World.x;
    Point3f_Perdicted_WorldAxis.y = y_predict;//coordinate_c2w.Axis_World.y;
    Point3f_Perdicted_WorldAxis.z = z_predict;//coordinate_c2w.Axis_World.z;

    if(fabsf(coordinate_w2c.Axis_Camera.x)<= 0.1 && fabsf(coordinate_w2c.Axis_Camera.y) <= 0.1 && coordinate_w2c.Axis_Camera.z <= 5)
        return;
    if (coordinate_w2c.Axis_Camera.z>200)//Transform
    {
        Transform(Point3f_Perdicted_WorldAxis, Gyro_Pitch_Align, 0, &VisionReceiveData.change_pitch, &VisionReceiveData.change_yaw,
                  &VisionReceiveData.delaytime,VisionYSpeed, GetBulletSpeed());
    }

    if(VisionControlFlag && AimAutoData.AutoAim){
        VisionChangeYaw = VisionReceiveData.change_yaw > VISIONMAXYAW ? VISIONMAXYAW : (VisionReceiveData.change_yaw < -VISIONMAXYAW ? -VISIONMAXYAW : VisionReceiveData.change_yaw);
        VisionChangePitch = VisionReceiveData.change_pitch > VISIONMAXPITCH ? VISIONMAXPITCH : (VisionReceiveData.change_pitch < -VISIONMAXPITCH ? -VISIONMAXPITCH : VisionReceiveData.change_pitch);

        GimbalYawSetTemp = VisionReceive.vision_data.horizon_x / 2 / PI - VisionChangeYaw / 360.0f;
        GimbalPitchSetTemp = VisionReceive.vision_data.vertical_y / PI - VisionChangePitch / 180.0f;

        yaw_set_last = yaw_set_gyro;
        yaw_set_gyro = VisionReceive.vision_data.horizon_x / 2 / PI - VisionChangeYaw / 360.0f;
        if(abs(yaw_set_gyro - yaw_set_last) > 0.2){
            return;
        }
        pitch_set_last = pitch_set_gyro;
        pitch_set_gyro = VisionReceive.vision_data.vertical_y / PI - VisionChangePitch / 180.0f;
        if(abs(pitch_set_gyro - pitch_set_last) > 0.2){
            return;
        }
    }

    vision_shoot_permit(); //弹频控制
}